#pragma once

#include "pins_config.h"
#include "NRF.h"
#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>
#include <Bounce2.h> // 按钮

struct TANK_KVS
{
  // 前端上面两个按键
  bool L_up = HIGH;
  bool R_up = HIGH;

  // int8_t 是有符号char型 -128 到 127
  int8_t LX = 0;
  int8_t LY = 0;
  int8_t RX = 0;
  int8_t RY = 0;

  // 8个功能按键
  bool up = HIGH;
  bool down = HIGH;
  bool left = HIGH;
  bool right = HIGH;
  bool o = HIGH;
  bool x = HIGH;
  bool a = HIGH;
  bool b = HIGH;

  // 4个拨杆开关    // 左1 --- 左2 --- 右1 --- 右2
  int sw_L1 = HIGH;
  int sw_L2 = HIGH;
  int sw_R1 = HIGH;
  int sw_R2 = HIGH;

  // 电位器旋钮
  int8_t L_knob = 0;
  int8_t R_knob = 0;
};

enum MOTOR_TYPE
{
  motor_stop,
  motor_forward,
  motor_backward,
  motor_left_drifting,
  motor_left_rotate,
  motor_right_drifting,
  motor_right_rotate,
  motor_left_trans,
  motor_right_trans,
};

struct SERVO_PARAM
{
  uint8_t angle;         // 当前角度
  uint8_t min_angle;     // 最小角度
  uint8_t max_angle;     // 最大角度
  uint8_t initial_angle; // 初始角度
};

class MotorControl
{
private:
  int8_t _angle_left = 30;  // 舵机1默认角度
  int8_t _angle_right = 20; // 舵机2默认角度
  int8_t _angle_up = 0;     // 舵机3默认角度
  int8_t _angle_down = 0;   // 舵机4默认角度
  int _step = 3;
  bool _isPressed_flag = false;

public:
  TANK_KVS tank_kvs;
  NRF nrf;

  SERVO_PARAM servo_left = {20, 10, 90, 20};
  SERVO_PARAM servo_right = {46, 10, 120, 46};
  SERVO_PARAM servo_up = {0, 0, 60, 0};
  SERVO_PARAM servo_down = {0, 0, 45, 0};

  Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(PCA9685_I2C_ADDRESS, Wire);

  MOTOR_TYPE motor_open_type = motor_stop; // 电机运行模式
  bool left_sw = false;                    // 电机是否左转 左漂移 左平移
  bool right_sw = false;                   // 电机是否右转 右漂移 右平移

  MotorControl();

  void init();
  // 读取收到的数据并写入tank_kvs
  void read();
  // 控制舵机
  void loadServo();
  // 控制电机
  void loadMotor();
  // 控制蜂鸣器
  void loadBuzzer();

  void clearSw();
  // 前进
  void forward(int speed);
  // 后退
  void backward(int speed);
  // 左旋转
  void left_rotate(int speed);
  // 右旋转
  void right_rotate(int speed);
  // 左漂移
  void left_drifting(int speed);
  // 左漂移
  void right_drifting(int speed);
  // 左平移
  void left_trans(int speed);
  // 右平移
  void right_trans(int speed);
  // 停止
  void stop();
  // 蜂鸣器
  void buzzer_set(int value);
  // 控制电机a
  void motor_a(int in1, int in2);
  // 控制电机b
  void motor_b(int in1, int in2);
  // 控制电机c
  void motor_c(int in1, int in2);
  // 控制电机d
  void motor_d(int in1, int in2);

  // 叉臂举起
  void raise_hand();
  // 叉臂向下
  void drop_hand();
  // 叉臂伸展
  void strech_hand();
  // 叉臂收回
  void shink_hand();
  // 夹子抓取
  void shake_hand();
  // 夹子松开
  void let_hand();

  void servo_Left_Set();
  void servo_Left_Set(int value);

  void servo_Right_Set();
  void servo_Right_Set(int value);

  void servo_Up_Set();
  void servo_Up_Set(int value);

  void servo_down_Set();
  void servo_down_Set(int value);

  void servo_angle(int pin, SERVO_PARAM obj, int angle);

  bool pressed(int key, bool default_state);
};